#include "TerrainModel.h"
#include "Triangle.h"
#include "Ray.h"
#include <map>

using namespace geom;

TerrainModel::TerrainModel()
{
	m_disp = geom::Vector3D(0,0,0);
	m_scale = 1.0;
}

TerrainModel::~TerrainModel()
{

}

void TerrainModel::setDisplacement(geom::Vector3D disp)
{
	m_disp = disp;
}
void TerrainModel::setScale(float scale)
{
	m_scale = scale;
}

/** Sets the base color. The color is in the range 0-1 for each of its components*/
void TerrainModel::setBaseColor(float r, float g, float b)
{
	m_baseColor[0] = r;
	m_baseColor[1] = g;
	m_baseColor[2] = b;
}


/** Forms a triangle in the mesh, based on 3 points that were inserted in it in a previous time */
void TerrainModel::addTriangle(const IntTri & indicesTri)
{
	m_tris.push_back(indicesTri);
    m_isGridUpToDate = false;
}

void TerrainModel::clearTriangles()
{
	m_tris.clear();
    m_isGridUpToDate = false;
}

/** Adds another point to the mesh. NOTE: It does not check if it has already been inserted */
void TerrainModel::addPoint(const TerrainPoint &p)
{
	m_points.push_back(p);
    m_isGridUpToDate = false;
}

void TerrainModel::addPoint(const geom::Point3D &p)
{
	TerrainPoint tp;
	tp.p = p;
	addPoint(tp);
    m_isGridUpToDate = false;
}

/** Changes a point
  @param pos Index of the point to change.
  @param p Point to change it to.*/
void TerrainModel::changePoint(int pos, const TerrainPoint &p)
{
	m_points[pos] = p;
    m_isGridUpToDate = false;
}

void TerrainModel::updateGrid() const
{
    geom::MinMax2DBox tbox(m_points[0].p);
    TerrainPointsList::const_iterator it = m_points.begin();
    TerrainPointsList::const_iterator end = m_points.end();
    for(;it!=end;++it)
        tbox.updateBoxWith(it->p);
    m_grid.clear();
    m_grid = geom::TriGrid(tbox, m_points.size(), 20);
    TrisList::const_iterator tit = m_tris.begin();
    TrisList::const_iterator tend = m_tris.end();
    for(;tit!=tend;++tit)
        m_grid.addTriangle(
                m_points[(*tit)[0]].p,
                m_points[(*tit)[1]].p,
                m_points[(*tit)[2]].p);

    m_isGridUpToDate = true;
}

float TerrainModel::getHeightAtCoords(float x, float y) const
{	
    if(!m_isGridUpToDate)
        updateGrid();

	return m_grid.getHeightAt((x - m_disp.dx)/m_scale,(y-m_disp.dy)/m_scale) * m_scale+ m_disp.dz ;
}

/** Generates the normals for the whole mesh (very slow) */
void TerrainModel::generateNormals()
{
    unsigned int numTris = m_tris.size();
    typedef std::multimap<unsigned int, geom::Vector3D> VectorsMMap;
    VectorsMMap normalsPerPoint;

    for(unsigned int i=0;i<numTris;++i)
    {
        const IntTri & tri = m_tris[i];
        geom::Triangle gtri(m_points[tri[0]].p,m_points[tri[1]].p,m_points[tri[2]].p);
        geom::Vector3D n = gtri.getNormal() * gtri.getArea();

        normalsPerPoint.insert(std::pair<unsigned int,geom::Vector3D>(tri[0], n));
        normalsPerPoint.insert(std::pair<unsigned int,geom::Vector3D>(tri[1], n));
        normalsPerPoint.insert(std::pair<unsigned int,geom::Vector3D>(tri[2], n));
    }

    while(normalsPerPoint.size()>0)
    {
        VectorsMMap::const_iterator it = normalsPerPoint.begin();
        unsigned int pointID = it->first;

        std::pair<VectorsMMap::iterator,VectorsMMap::iterator> range = normalsPerPoint.equal_range(pointID);

        geom::Vector3D sum(0,0,0);
        for(it=range.first;it!=range.second;++it)
            sum+=it->second;
        m_points[pointID].n = sum.getNormalized();
        normalsPerPoint.erase(range.first, range.second);
    }
}


